;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;                                                                            ;;
;; Part one of the system initialization code, contains low-level             ;;
;; initialization.                                                            ;;
;;                                                                            ;;
;; Copyright 2006 IAR Systems. All rights reserved.                           ;;
;;                                                                            ;;
;; $Revision: 21638 $                                                         ;;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;


;-------------------------------------------------------------------------------
; Macros and definitions for the whole file
;-------------------------------------------------------------------------------

        MODULE  ?cstartup

        ;; Forward declaration of sections.
        SECTION IRQ_STACK:DATA:NOROOT(3)
        SECTION FIQ_STACK:DATA:NOROOT(3)
        SECTION SVC_STACK:DATA:NOROOT(3)
        SECTION ABT_STACK:DATA:NOROOT(3)
        SECTION UND_STACK:DATA:NOROOT(3)
        SECTION CSTACK:DATA:NOROOT(3)

;-------------------------------------------------------------------------------
; The module in this file are included in the libraries, and may be
; replaced by any user-defined modules that define the PUBLIC symbol
; __iar_program_start or a user defined start symbol.
;
; To override the cstartup defined in the library, simply add your
; modified version to the workbench project.
;-------------------------------------------------------------------------------

        SECTION .intvec:CODE:NOROOT(2)

        PUBLIC  __vector
        PUBLIC  __iar_program_start
        PUBLIC  __vector_0x14

        EXTERN  tn_cpu_irq_isr
        ;EXTERN  tn_startup

        ARM                         ;; Always ARM mode after reset

;-------------------------------------------------------------------------------
; Vectors table
;-------------------------------------------------------------------------------
__vector:
        ldr pc,[pc,#24] ;; Absolute jump can reach 4 GByte
                
__undef_handler:
        ldr pc,[pc,#24] ;; Branch to undef_handler
    
__swi_handler:
        B .             ;; Branch to swi_handler
                
__prefetch_handler:
        ldr pc,[pc,#24] ;; Branch to prefetch_handler
                
__data_handler
        ldr pc,[pc,#24] ;; Branch to data_handler
                
__vector_0x14
                DC32    0xFFFFFFFF
                
__irq_handler:
        ldr pc,[pc,#24] ;; Branch to irq_handler
                
__fiq_handler:
        ldr pc,[pc,#24] ;; Branch to fiq_handler


                DC32  __iar_program_start       ;; Reset
                DC32  undef_handler             ;; Undefined instructions
                DC32  0                         ;; Software interrupt (SWI/SVC)
                DC32  prefetch_handler          ;; Prefetch abort
                DC32  data_handler              ;; Data abort
                DC32  0                         ;; RESERVED
                DC32  tn_cpu_irq_isr            ;; IRQ    (+24)
                DC32  fiq_handler               ;; FRQ    (+28)


;-------------------------------------------------------------------------------
; Mode, correspords to bits 0-5 in CPSR
;-------------------------------------------------------------------------------
MODE_MSK DEFINE 0x1F            ;; Bit mask for mode bits in CPSR
USR_MODE DEFINE 0x10            ;; User mode
FIQ_MODE DEFINE 0x11            ;; Fast Interrupt Request mode
IRQ_MODE DEFINE 0x12            ;; Interrupt Request mode
SVC_MODE DEFINE 0x13            ;; Supervisor mode
ABT_MODE DEFINE 0x17            ;; Abort mode
UND_MODE DEFINE 0x1B            ;; Undefined Instruction mode
SYS_MODE DEFINE 0x1F            ;; System mode

NOINT    DEFINE 0xc0
IRQ_BIT  DEFINE 0x80            ;; when IRQ bit is set, IRQ is disabled
FIQ_BIT  DEFINE 0x40            ;; when FIQ bit is set, FIQ is disabled


;-------------------------------------------------------------------------------
;
;-------------------------------------------------------------------------------
        SECTION .CSTARTUP:CODE:NOROOT(2)

        EXTERN  ?main
        REQUIRE __vector
        EXTERN  low_level_init

        ARM

;-------------------------------------------------------------------------------
; Handlers
;-------------------------------------------------------------------------------
undef_handler:
          B .

prefetch_handler:
          B .

data_handler:
          B .

fiq_handler:
          B .

;-------------------------------------------------------------------------------
;
;-------------------------------------------------------------------------------
__iar_program_start:
                ?cstartup:


;; Disable all interrupts
                LDR   R0,=0xFFFFF014
                MOV   R1,#0xFFFFFFFF
                STR   R1,[R0]


;-------------------------------------------------------------------------------
; Remap Exption Vectors to Internal RAM
;-------------------------------------------------------------------------------          
               ;; Copy int vectors from flash to ram
               ;; 64 bytes copy need
          
VECTORS  EQU 0x00010000
RAM_BASE EQU 0x40000000
MEMMAP   EQU 0xE01FC040

               LDR R8, =VECTORS
               LDR R9, =RAM_BASE
               
               LDMIA R8!, {R0-R7}
               STMIA R9!, {R0-R7}
               LDMIA R8!, {R0-R7}
               STMIA R9!, {R0-R7}
   
               LDR   R0, =MEMMAP
               MOV   R1, #+2
               STRB  R1, [R0]

;-------------------------------------------------------------------------------
; Execution starts here.
; After a reset, the mode is ARM, Supervisor, interrupts disabled.
; Initialize the stack pointers.
; The pattern below can be used for any of the exception stacks:
; FIQ, IRQ, SVC, ABT, UND, SYS.
; The USR mode uses the same stack as SYS.
; The stack segments must be defined in the linker command file,
;
; Set stack pointers
;-------------------------------------------------------------------------------

                MRS    R0, cpsr                            ;; Original PSR value
                BIC    R0, R0, #MODE_MSK                   ;; Clear the mode bits
                ORR    R0, R0, #FIQ_MODE                   ;; Set FIR mode bits
                MSR    CPSR_C, R0                          ;; Change the mode
                LDR    SP,=SFE(FIQ_STACK)                  ;; End of FIQ_STACK

                BIC    R0,R0,#MODE_MSK                     ;; Clear the mode bits
                ORR    R0,R0,#IRQ_MODE                     ;; Set IRQ mode bits
                MSR    CPSR_C, R0                          ;; Change the mode
                LDR    SP,=SFE(IRQ_STACK)                  ;; End of IRQ_STACK

                BIC    R0, R0, #MODE_MSK                   ;; Clear the mode bits
                ORR    R0, R0, #(SVC_MODE|IRQ_BIT|FIQ_BIT) ;; Set Supervisor mode bits
                                                           ;; and dis int
                MSR    CPSR_C, R0                          ;; Change the mode
                LDR    SP,=SFE(CSTACK)                     ;; End of CSTACK

;-------------------------------------------------------------------------------
;
;-------------------------------------------------------------------------------
#ifdef __ARMVFP__
; Enable the VFP coprocessor.

                MOV    R0, #BASE_ARD_EIM                   ;; Set EN bit in VFP
                FMXR   FPEXC, R0                           ;; FPEXC, clear others.

;; Disable underflow exceptions by setting flush to zero mode.
;; For full IEEE 754 underflow compliance this code should be removed
;; and the appropriate exception handler installed.

                MOV    R0, #0x01000000                     ;; Set FZ bit in VFP
                FMXR   FPSCR, R0                           ;; FPSCR, clear others.
#endif

;-------------------------------------------------------------------------------
; Add more initialization here
;-------------------------------------------------------------------------------


;-------------------------------------------------------------------------------
; Continue to ?main for more IAR specific system startup
;-------------------------------------------------------------------------------

                LDR    R0,=?main
                BX     R0


                END